package trb.complexshield.bool;

import com.sun.j3d.utils.geometry.Cylinder;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.Tuple3f;
import javax.vecmath.Vector3f;
import no.hrp.hvrc.rad.calc.intersect.Segment;
import no.hrp.hvrc.utils.j3d.BBox;
import no.hrp.hvrc.utils.j3d.Mat4;
import no.hrp.hvrc.utils.j3d.Vec3;

/**
 * A cylinder aligned at the y axis. The cylinders endpoints are at
 * (0, 0, 0) and (0, height, 0).
 *
 * @author tomrbryn
 */
public class IntersectableCylinder implements SegmentIntersectable {
    private static final float EPSILON = 0.0001f;

    // center of cylinder in the xz plane
    private float r;

    // height of the cylinder
    private float height;

    // the shields local to world matrix
    private final Transform3D local2World = new Transform3D();

    // the world to local space transformation matrix
    protected Transform3D world2local = new Transform3D();

    // temp var, the transformed line
    private static final ThreadLocal<Segment> lThreadLocal = new ThreadLocal<Segment>() {
        @Override
        protected Segment initialValue() {
            return new Segment(new Vector3f(), new Vector3f(0, 1, 0));
        }
    };

    private BBox aabb;

    /**
     * Creates a cylinder with the specified radius and height
     * @param r the radius
     * @param height the height
     * @param t3d the local2world transform
     */
    public IntersectableCylinder(float r, float height, Transform3D t3d) {
        this.r = r;
        this.height = height;
        world2local.invert(t3d);
        local2World.set(t3d);
        aabb = new BBox(new Vec3(-r, 0, -r), new Vec3(r, height, r));
        aabb.transform(local2World);
    }

    public float getHeight() {
        return height;
    }

    public float getRadius() {
        return r;
    }

    public Mat4 getLocalToWorld() {
        return new Mat4(local2World);
    }

    public Mat4 getWorldToLocal() {
        return new Mat4(world2local);
    }

    @Override
    public boolean intersects(Segment line, float[] tMinMaxOut) {
        Segment l = lThreadLocal.get();
        l.transform(line, world2local);
        //System.out.println("transform line: "+l);

        // set min and max distance to match line
        float tmin = 0;
        float tmax = l.length();
        float ooLength = 1f / tmax;

        float invDirectionY = 1f / l.directionY();
        float signY = (invDirectionY < 0 ? 1 : 0);

        // calculate intersection between line and y == 0 and y == height
        float tymin = ((     signY  * height) - l.originY()) * invDirectionY;
        float tymax = (((1 - signY) * height) - l.originY()) * invDirectionY;

        // check if line is outside slab
        if ((tmin > tymax) || (tymin > tmax)) {
            return false;
        }

        //System.out.println(""+tmin+" "+tmax+" "+tymin+" "+tymax);

        // cap line to slab
        if (tmin < tymin) {
            tmin = tymin;
        }
        if (tmax > tymax) {
            tmax = tymax;
        }

        // check if line is prallel to cylinder axis
        if (l.directionY() >= (1f - EPSILON)) {
            //System.out.println("line is paralell to axis.");
            // determine if the line intersects the cylinder end disks.
            if ((l.originX() * l.originX()) + (l.originZ() * l.originZ()) > (r * r)) {
                // line lies outside disks
                //System.out.println("line do not intersect disks.");
                return false;
            }

            float tdif = tmax - tmin;
            tMinMaxOut[0] = tmin * ooLength;
            tMinMaxOut[1] = tmax * ooLength;
            return tdif > 0;
        }

        // intersect line against circle in the x, z plane
        // Test intersection of line P+t*D with infinite cylinder
        // x^2+y^2 = r^2.  This reduces to computing the roots of a
        // quadratic equation.  If P = (px,py,pz) and D = (dx,dy,dz),
        // then the quadratic equation is
        //   (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
        float kPX = l.originX();
        float kPY = l.originZ();
        float kDX = l.directionX();
        float kDY = l.directionZ();
        float a = kPX*kPX + kPY*kPY - r*r;
        float b = kPX*kDX + kPY*kDY;
        float c = kDX*kDX + kDY*kDY;
        float fDiscr = b*b - a*c;
        if (fDiscr <= 0.0) {
            // line tangents or does not intersect cylinder
            return false;
        }

        // line intersects cylinder in two places
        float root = (float) Math.sqrt(fDiscr);
        float inv = 1.0f / c;
        float sircleMin = (-b - root) * inv;
        float sircleMax = (-b + root) * inv;

        //System.out.println("" + sircleMin+" "+sircleMax);

        // cap line to slab
        if (tmin < sircleMin) {
            tmin = sircleMin;
        }
        if (tmax > sircleMax) {
            tmax = sircleMax;
        }

        float tdif = tmax - tmin;
        tMinMaxOut[0] = tmin * ooLength;
        tMinMaxOut[1] = tmax * ooLength;
        return tdif > 0;
    }

    /**
     * Creates a cylinder from the material, endpoints and radius
     * @param p1 cylinder endpoint
     * @param p2 cylinder endpoint
     * @param r radius
     * @return the created CylinderShield
     */
    public static IntersectableCylinder fromEndpoints(Tuple3f p1, Tuple3f p2, float r) {
        Vector3f yVec = new Vector3f();
        yVec.sub(p2, p1);
        float height = yVec.length();
        if (height == 0) {
            throw new IllegalArgumentException("Cylinder endpoint must not be the same.");
        }
        yVec.normalize();
        Vector3f xVec = new Vector3f(1, 0, 0);
        if (Math.abs(yVec.x) > 0.9) {
            xVec.set(0, 1, 0);
        }
        Vector3f zVec = new Vector3f();
        zVec.cross(xVec, yVec);
        xVec.cross(yVec, zVec);
        float[] m = new float[] {
                xVec.x, xVec.y, xVec.z, p1.x,
                yVec.x, yVec.y, yVec.z, p1.y,
                zVec.x, zVec.y, zVec.z, p1.z,
                0,      0,      0,      1
        };

        Transform3D t3d = new Transform3D(m);
        return new IntersectableCylinder(r, height, t3d);
    }

    /**
     * Creates a Java3D geometry that matches the cylinder.
     * @return the geometry
     */
    public BranchGroup createGeometry() {
        Cylinder cylinder = new Cylinder(r, height);

        // adjust from center in origo to bottom in origo
        Transform3D t3d = new Transform3D();
        t3d.mulInverse(world2local);

        Transform3D t3d2 = new Transform3D();
        t3d2.setTranslation(new Vector3f(0, height/2, 0));
        t3d.mul(t3d2);

        TransformGroup tg = new TransformGroup(t3d);
        tg.addChild(cylinder);
        BranchGroup bg = new BranchGroup();
        bg.addChild(tg);
        return bg;
    }

    public BBox getAABB() {
        return aabb;
    }
}